#include "ros/ros.h"
#include "Eigen/Eigen"
#include <geometry_msgs/PoseStamped.h>
using namespace std;

#define PI_2 1.5708

int main()
{

	geometry_msgs::PoseStamped* msg=new geometry_msgs::PoseStamped();
	msg->pose.position.x=1.0;
	msg->pose.position.y=0;
	msg->pose.position.z=0;

	double theta=90/180.0*3.1415926;
	msg->pose.orientation.w=cos(theta/2);
	msg->pose.orientation.x=sin(theta/2);
	msg->pose.orientation.y=0;
	msg->pose.orientation.z=0;


    // t265 的数据是机体NWU相对于大地NWU，将其转换为机体ENU相对于大地ENU
    Eigen::Vector3d T2652T265_ORIGIN_translation(-msg->pose.position.y,msg->pose.position.x,msg->pose.position.z);
    Eigen::Quaterniond T2652T265_ORIGIN_rotation(msg->pose.orientation.w,msg->pose.orientation.x,
														msg->pose.orientation.y,msg->pose.orientation.z);
    Eigen::Quaterniond NWU2ENU(cos(PI_2/2.0),0,0,sin(PI_2/2.0));
    T2652T265_ORIGIN_rotation=NWU2ENU*T2652T265_ORIGIN_rotation*NWU2ENU.inverse();


	ROS_INFO("T2652T265_ORIGIN_translation : %f %f %f",T2652T265_ORIGIN_translation(0),T2652T265_ORIGIN_translation(1),T2652T265_ORIGIN_translation(2));

	ROS_INFO("T2652T265_ORIGIN_rotation w x y z : %f %f %f %f",T2652T265_ORIGIN_rotation.w(),T2652T265_ORIGIN_rotation.x(),T2652T265_ORIGIN_rotation.y(),
																			T2652T265_ORIGIN_rotation.z());


    double D435_T265_OFFSET_X=0.0;     //D435在T265前方为正
    double D435_T265_OFFSET_Z=0.1;     //D435在T265上方为正

    double D435_BASE_OFFSET_X=0.0;     //D435在云台基座前方为正
    double D435_BASE_OFFSET_Z=0.0;    //D435在云台基座上方为正

    double T265_OFFSET_X=2.0;        //T265在无人机中心前方为正
    double T265_OFFSET_Y=0.0;        //T265在无人机中心左边为正
    double T265_OFFSET_Z=0.0;        //#T265在无人机中心上方为正


	Eigen::Matrix4d T2652vehicle = Eigen::Matrix4d::Identity();
	T2652vehicle.block(0, 0, 3, 3) = Eigen::Matrix3d::Identity();
	T2652vehicle(0, 3) = T265_OFFSET_X;
	T2652vehicle(1, 3) = T265_OFFSET_Y;	//left direction
	T2652vehicle(2, 3) = T265_OFFSET_Z;  //up direction

	Eigen::Matrix4d T2652T265_ORIGIN = Eigen::Matrix4d::Identity();
	T2652T265_ORIGIN.block(0, 0, 3, 3) =  Eigen::Matrix3d(T2652T265_ORIGIN_rotation);
	T2652T265_ORIGIN(0, 3) = T2652T265_ORIGIN_translation(0);
	T2652T265_ORIGIN(1, 3) = T2652T265_ORIGIN_translation(1);	//left direction
	T2652T265_ORIGIN(2, 3) = T2652T265_ORIGIN_translation(2);  //up direction


	Eigen::Matrix4d transform = T2652vehicle*T2652T265_ORIGIN*T2652vehicle.inverse();

    Eigen::Vector3d current_p;
	current_p << transform(0,3),transform(1,3),transform(2,3);

	ROS_INFO("current_p: %f %f %f",current_p(0),current_p(1),current_p(2));




}